#include "ros/ros.h"
#include "ros/package.h"
#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <fstream>


#include <tf/transform_broadcaster.h>




namespace px4_operator_vision_imu_vo_pose_sender
{
using namespace Eigen;
using namespace std;


const int CALIBRATION_RECORD_NUM = 100;

int calibration_record_uav_count = 0;
int calibration_record_device_count = 0;
Matrix3d calibration_record_att_device[CALIBRATION_RECORD_NUM];
Matrix3d calibration_record_att_uav[CALIBRATION_RECORD_NUM];
bool calibrate_att_device_uav = false;
double last_send_timestamp = 0;
double send_interval = 0.05;
ros::Publisher vision_pos_pub;
tf::TransformBroadcaster *tf_uav_enu_pub_ptr;

Affine3d pose_uav_device;

void BroadcastTF(tf::TransformBroadcaster * tfBroadcaster_ptr, const Affine3d& pose,const char* world, const char* tag)
{
	Eigen::Quaterniond pose_R_quat = Eigen::Quaterniond(pose.rotation());
	Eigen::Vector3d pose_t = pose.translation();
	tf::Transform tf_uav_enu;
	tf_uav_enu.setOrigin(tf::Vector3(pose_t.x(),pose_t.y(),pose_t.z()));
	tf_uav_enu.setRotation(tf::Quaternion(
			pose_R_quat.x(),
			pose_R_quat.y(),
			pose_R_quat.z(),
			pose_R_quat.w()
	));
	tfBroadcaster_ptr->sendTransform(tf::StampedTransform(tf_uav_enu,ros::Time::now(), world, tag));
}




void tf_callback(const tf::tfMessage::ConstPtr& msg)
{

	for(int i=0;i<msg->transforms.size();i++)
	{
		if(msg->transforms[i].header.frame_id == "global_enu" && msg->transforms[i].child_frame_id == "device_rhu")
		{
			Affine3d pose_device_enu;
			tf::transformMsgToEigen(msg->transforms[i].transform, pose_device_enu);

			if(!calibrate_att_device_uav)
			{
				geometry_msgs::PoseStamped new_msg;
				new_msg.header.stamp = ros::Time::now();
				Affine3d pose_uav_enu = pose_device_enu * pose_uav_device;
				tf::poseEigenToMsg(pose_uav_enu,new_msg.pose);

				double time_current = ros::Time::now().toSec();
				if(time_current - last_send_timestamp >= send_interval)
				{
					vision_pos_pub.publish(new_msg);
					last_send_timestamp = time_current;
				}
			}
			else if(calibration_record_device_count < CALIBRATION_RECORD_NUM)
			{
				//cout<<pose_device_enu.matrix()<<endl;
				calibration_record_att_device[calibration_record_device_count] = pose_device_enu.rotation();
				calibration_record_device_count ++;
				if(calibration_record_device_count % 10 == 0)
				{
					ROS_INFO("calibration_record_device_count = %d", calibration_record_device_count);
				}
			}
			break;
		}
	}
}

void uav_pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	Affine3d uav_local_pose;
	tf::poseMsgToEigen(msg->pose,uav_local_pose);
	BroadcastTF(tf_uav_enu_pub_ptr,uav_local_pose,"global_enu", "uav_hlu");
	if(calibrate_att_device_uav && calibration_record_uav_count < CALIBRATION_RECORD_NUM)
	{
		//cout<<uav_local_pose.matrix()<<endl;
		calibration_record_att_uav[calibration_record_uav_count] = uav_local_pose.rotation();
		calibration_record_uav_count ++;
		if(calibration_record_uav_count % 10 == 0)
		{
			ROS_INFO("calibration_record_uav_count = %d", calibration_record_uav_count);
		}

	}

}


}


using namespace px4_operator_vision_imu_vo_pose_sender;

int main(int argc, char **argv)
{
	ros::init(argc, argv, "vision_imu_vo_pose_sender");
	ros::NodeHandle nh;

	ros::param::get("~calibrate", calibrate_att_device_uav);
	ros::param::del("~calibrate");


	string transform_file;
	transform_file = ros::package::getPath("px4_operator");
	transform_file += "/vision_imu_device_to_uav_transform.txt";
	ros::param::get("~transform_file",transform_file);
	ros::param::del("~transform_file");

	ROS_INFO("device to uav transform file is set to %s",transform_file.c_str());



	vision_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/mocap/pose", 10);
	ros::Subscriber tf_sub = nh.subscribe("/tf", 5, tf_callback);
	ros::Subscriber uav_pose_sub = nh.subscribe("/mavros/local_position/pose", 10, uav_pose_callback);

    tf_uav_enu_pub_ptr = new tf::TransformBroadcaster();

	double send_freq = 40.0;
	ros::param::get("~send_freq", send_freq);
	ros::param::del("~send_freq");

	if(send_freq <=0) send_freq = 40.0;
	else if(send_freq <= 2) send_freq = 2;
	else if(send_freq >= 1000) send_freq = 1000;
	send_interval = 1.0 / send_freq;



	/*
	while(ros::ok())
	{
		BroadcastTF(tf_uav_enu_pub_ptr,pose_device_uav,"uav_hlu", "device_rhu");
		ros::spinOnce();
		usleep(20*1000);
	}
	*/


	if(calibrate_att_device_uav)
	{


		while(ros::ok()&&(calibration_record_device_count<CALIBRATION_RECORD_NUM || calibration_record_uav_count < CALIBRATION_RECORD_NUM))
		{
			ros::spinOnce();
		}
		if(calibration_record_device_count == CALIBRATION_RECORD_NUM && calibration_record_uav_count == CALIBRATION_RECORD_NUM)
		{

			double sum_x,sum_y,sum_z,sum_w;
			sum_x=sum_y=sum_z=sum_w=0;
			for(int i=0;i<CALIBRATION_RECORD_NUM;i++)
			{
				Matrix3d curr_pose_uav_device = calibration_record_att_uav[i].inverse() * calibration_record_att_device[i];
				Quaterniond curr_pose_uav_device_quat = Quaterniond(curr_pose_uav_device);
				sum_x += curr_pose_uav_device_quat.x();
				sum_y += curr_pose_uav_device_quat.y();
				sum_z += curr_pose_uav_device_quat.z();
				sum_w += curr_pose_uav_device_quat.w();

			}
			Quaterniond avg_quat;
			avg_quat.x() = sum_x / CALIBRATION_RECORD_NUM;
			avg_quat.y() = sum_y / CALIBRATION_RECORD_NUM;
			avg_quat.z() = sum_z / CALIBRATION_RECORD_NUM;
			avg_quat.w() = sum_w / CALIBRATION_RECORD_NUM;
			avg_quat.normalize();

			Matrix3d att_device_uav = avg_quat.toRotationMatrix();


			cout<<setprecision(15)<<att_device_uav<<endl;

			ofstream transform_file_ofs(transform_file.c_str());


			if(!transform_file_ofs.is_open())
			{
				ROS_ERROR("Fail to open transform file %s", transform_file.c_str());
				return 1;
			}

			transform_file_ofs<<setprecision(15)<<att_device_uav<<endl;

			transform_file_ofs.close();
		}


	}
	else
	{
		ifstream transform_file_ifs(transform_file.c_str());
		if(!transform_file_ifs.is_open())
		{
			ROS_ERROR("Fail to open transform file %s", transform_file.c_str());
			return 1;
		}
		double transform_attitude_params[9];
		for(int i=0;i<9;i++)
		{
			transform_file_ifs>>transform_attitude_params[i];
		}
		transform_file_ifs.close();

		Affine3d pose_device_uav = Affine3d::Identity();

		pose_device_uav.linear()<<transform_attitude_params[0],transform_attitude_params[1],transform_attitude_params[2],
				transform_attitude_params[3],transform_attitude_params[4],transform_attitude_params[5],
				transform_attitude_params[6],transform_attitude_params[7],transform_attitude_params[8];

		pose_device_uav.translation()<< -0.0545, 0.0860, -0.0428;

		pose_uav_device = pose_device_uav.inverse();

		cout<<pose_uav_device.matrix()<<endl;






		ros::spin();
	}


	delete(tf_uav_enu_pub_ptr);
	return 0;
}


